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ABSTRACT. This paper deals with the class of problems that involve finding where to place or how 
to move a solid object in the presence of obstacles. The solution to this class of problems is essential 
to the automatic planning of manipulator transfer movements, i.e. the motions to grasp a part and 
place it at some destination. This paper presents algorithms for planning manipulator paths that 
avoid collisions with objects in the workspace and for choosing safe grasp points on objects. These 
algorithms allow planning transfer movements for cartesian manipulators. The approach is based on 
a method of computing an explicit representation of the manipulator configurations that would bring 
about a collision [27]. 
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1. Introduction 

An important goal of research on programming languages for computer-controlled manipulators 
is a language in which assembly operations can be concisely described. Two major approaches to 
manipulator programming have been identified [34]: 

1. Explicit programming — in which die user specifies all the manipulator motions needed to 
accomplish a task. 

2. Model-based programming — in which the user specifies geometric models of parts and a 
description of the task in terms of diese models. Hie detailed manipulator motions are derived 
by die assembly system from these specifications. 

This paper presents algorithms for some of the central geometric problems that arise in die model- 
based approach to manipulator programming. In particular, this paper deals with the class of 
problems that involve finding where to place or how to move a solid object in the presence of 
/" s obstacles. The solution to this class of problems is essential to the automatic planning of manipulator 

transfer movements, i.c, the motions to grasp a part and place it at some destination. For example, 
planning transfer movements requires the ability to plan padis for the manipulator diat avoid colli- 
sions with objects in the workspace and the ability to choose safe grasp points on objects. The ap- 
proach to Uicsc problems described here is based on a method of computing an explicit representation 
of the manipulator configurations that would bring about a collision [27]. 

Several model-based manipulator systems have been described in the recent literature: AL [10] 
[46], Autopass [24] [28] [50], LAMA [25] [26] and RAFF [38] [39] [40]. These are experimental systems, 
currently under development 1 . Work on die model-based aspects of AL has focused on techniques 
for making coding decisions in manipulator programs. Hie decisions arc made among a fixed set of 
strategies so as to minimize estimated execution times and so as to bring estimates on the accuracy 
of part positions within specified bounds. A central technical issue in this approach is deriving the 
accuracy estimates from geometric relationships and local accuracy information. RAFf has focused 
1 on the specification of manipulator programs by specifying the desired symbolic spatial relation- 
ships among objects. These relations are Uicn translated into algebraic constraints on the position 

/ 0m \ i-Thc AL language, as originally described, includes explicit as well as model-based programming capabilities. The former 

are currently available, while the latter are still in the experimental stage. 
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parameters of the objects, which can be solved by symbolic manipulation. These algebraic solution 
techniques are also used to complete the specification of partially specified actions so as to achieve 
die desired relationships. Implementation work on LAMA and Autopass has focused on techniques 
for planning collision-free motions, e.g. grasping and parts transfer motions, using polyhedral object 
models. The techniques reported in this paper are extensions of the Autopass obstacle avoidance 
algorithm and LAMA's grasping strategies. 

A number of important problems relevant to model-based manipulator programming have been 
addressed independently of any manipulator system, for example the problem of specifying com- 
pliant motion strategies based on geometric and kinematic models of a task [30], die selection of 
grasping positions [5] [31] [35] [51], and the problem of collision detection and collision avoidance 
among obstacles [3] [7] [12] [33] [47]. 

The algorithms discussed in this paper are based on previous work on obstacle avoidance algo- 
rithms. In particular, [48] [49] first formulated the obstacle avoidance problem in terms of an obstacle 
transformation which allows treating the moving object as a point. A similar transformation was also 
used in [1] [2] [4] [45] for the template layout problem; related applications arc also discussed in [11] 
[16]. Generalizations of these obstacle transformation techniques and a review of related work can be 
found in [27] and [28]. Other approaches to automatic obstacle avoidance arc reviewed in [23] [48]. 



2. The "Pick and Place" Synthesis Problem 

The most common transfer movements are of die "pick and place 11 type, consisting of (1) moving 
the manipulator from its current configuration 2 to a grasp configuration on some object, P, (2) grasp- 
ing P, and (3) moving P to some specified configuration. The "pick and place" synthesis problem is 
that of deriving the manipulator motions that will carry out a "pick and place" transfer movement, 
given as input the following data: 
.1. a geometric description of the manipulator and the objects in the workspace, 

2. die current configurations of the manipulator and the objects in the workspace, 



2 Configuration will be used here to refer to the combined position and orientation of an object as well as to the set 
of joint parameters specifying the arrangement of manipulator links. 
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3. the desired final configuration of P, and 
4 (Optional) the grasp configuration on P. 

This paper focuses on the geometric aspects of the "pick and place" synthesis problem. For 
example, when the grasp configuration is known, the "pick and place" synthesis problem is equivalent 
to finding collision-free paths for the manipulator and P between the configurations in items 2, 3 
and 4 above; when the grasp configuration is unknown, there is the additional task of choosing a 
configuration such that: 

1. the manipulator's fingers are in contact with P, 

2. the manipulator does not collide with nearby objects, 

3. the configuration is reachable, and 

4. the object is stable in the manipulator's hand. 

The first three conditions reflect geometric constraints on the manipulator configuration, relative to P 
and to other objects in the workspace. 'Hie stability condition reflects aspects of grasping beyond the 
purely geometric, but when P is smajl relative to the manipulator hand and when parts mating effects 
arc ignored, then stability considerations can typically be reduced to geometric heuristics (sec Section 
9.6). 

The geometric aspects of "pick and place" can be formulated in terms of two fundamental spatial 
planning problems [27], Findspace and Findpath, which occur in many applications. The definition of 
these basic problems are presented below for the case of polyhedral objects. 

Let R be a convex polyhedron that bounds the workspace and which contains ko other, possibly 
overlapping, convex polyhedra Bj designated as obstacles. Let A, the object being moved, be the 
union of Ica convex polyhedra A^ i.e. A = Ui=i M* 

1. Findspace — Find a configuration for A, inside P, such that ViVj ; A{ O Bj = 0. This is 
called a safe configuration. 

1 Findputh — Find a path for A from configuration 8 to configuration g such that A is always 
in R and A never overlaps any of the Bj. This is called a safe path. 
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Clearly, "pick and place 1 ' with known grasp configuration can be viewed as a sequence of two 
Findpath problems. In addition, the configurations which are legal candidates For grasping can be 
derived from solutions to the Findspace problem. 

The reduction of die "pick and place" problem to diese more fundamental geometric problems 
assumes that the locations of all objects are known to high accuracy and that the path of the 
manipulator can be controlled to the same precision. In a realistic environment, there is always uncer- 
tainty in die positions of objects and error in the control of the manipulator. Section 10 discusses the 
effects of uncertainty. 

3. The Cspace Approach to Spatial Planning: Overview 

In this section, an overview of the Configuration Space approach to spatial planning will be 
presented; further details can be found in [27]. 

Hie position and orientation of a rigid solid can be specified by a single 6-dimcnsional vector, 
called its configuration. The 6-dimensional space of configurations for a solid, A, is called its 
Configuration Space and denoted Cspace A . For example, a configuration may have one coordinate 
value for each of die x, y, z coordinates of a selected point on the object and one coordinate value for 
each of the object's Eulcr angles [21], In general, an n-dimensional configuration space can be used 
to model any system for which the position of every point on die objcct(s) can be specified with n 
parameters. An example is the configuration of an industrial robot with n joints, where n is typically 5 
or 6. In CspaceA, the set of configurations of A where A overlaps B\ i.e. A n B j£ 0, will be denoted 
CO A {B), die Cspace^ Obstacle due to B. Similarly, those configurations of A where A is completely 
inside B, i.e. A C'j3, will be denoted CU{B) 9 the C space A Interior of B. Together, these two 
Cspace A constructs embody all the information needed to solve Findspace and Findpath problems. 
Note Uiat CI A (B) — —COa{—B), where — X denotes the set complement of X in #. 

3.1. Fixed Orientation of A 

In two dimensions, if the orientation of a convex polygon A is fixed, C space A is simply die [x } y) 
plane. This is so because the (x, y) position of some reference vertex rv A is sufficient to specify the 
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Figure 1. The C»pnce A obstacle due lo H, for Axed orientation of A. 



polygon's configuration. In this case, the presence of another convex polygon B constrains rv A to 
be outside of CO A (B), a larger convex polygon, shown as the shaded region in Figure 1. Since 
CO A {B\ in this case, is a set of (z, y) values, it is denoted CO% J (B), Similarly, if A and B arc three- 
dimensional polyhedra in fixed orientations, then the C space obstacles arc denoted CO% JZ (B). Thus, 
the Findspace problem for polygons, with fixed orientation, can be transformed to the equivalent 
problem of placing rv A outside of CO*?{B\ but inside CI^ V {R). Similarly, for multiple obstacles Bj, 
a location for A is safe if and only if rv A is not inside any of the CO'^(Bj) t but is inside CI^[R). 

If the orientation of A is fixed, then the Findpath problem for the polygon A among the Bj is 
equivalent to the Findpath problem for the point rv A among the CO% J (Bj). When die CO x ^{B 3 ) 
are polygons, die shortest 3 safe paths for rv A are piccewise linear paUis connecting die start and the 
goal via die vertices of the CO% J (B) polygons, Figure 2. Therefore, Findpath can be formulated as a 
graph search problem. The graph is formed by connecting all pairs of vertices of Cspace A obstacles 
(and the start and goal) that can "see" each other,, i.e. can be connected by a straight line that does 
not intersect any of the obstacles. The shortest path from the start to the goal in this visibility graph 



3 This assumes Euclidean distance as a metric. For the optimaiity conditions using a rectilinear (Manhattan) metric, see 
[22]. 
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Figure 2. This figure illustrates the Findpath problem and its formulation using C 8 pace obstacles. Note 
that the shortest collision- free paths connect the origin and the destination via the vertices of the C space A 
obstacles. 



(Vgraph) is the shortest safe path for A among the Bj [28]. This algorithm solves Findpath problems 
when the orientation of A is fixed. But, because they require moving A along obstacle boundaries, 
shortest paths are very susceptible to inaccuracies in the object models. 

The approach to Findspace and Findpath described above generalizes to problems involving 
'three dimensional polyhedra with fixed orientation. The generalization requires the use of a three- 
dimensional Cspace A , representing the space of [x } y } z) positions of rv A . In this Cspace, the 
obstacles are also polyhedra, denoted CO%* z (B). However, the Vgraph algorithm. has several addi- 
tional drawbacks when' the obstacles are three-dimensional:' 

1. Shortest paths do not typically traverse the vertices of the CO^ z (Bj), 

2. .Thcrb may be no paths via vertices, within the enclosing polyhedral region R y although 
other types of safe paths within R may exist. 



These drawbacks may be alleviated by introducing additional nodes in the Vgraph which do not 
correspond to vertices [28]. An alternative strategy for finding safe paths among two- or three- 
dimensional C&pactA obstacles is discussed in Section 7. 
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3.2. AlgoritiimsforCO^ z {B) 

The central operation in the Cspace approach to Hnd.space and Findpath in two and three 
dimensions is computing CO* x y (B) and CO* x i,s (B) respectively. If A and B arc convex polyhcdra, it is 
simple to show [27] that 

CO%'*{B) = BQ (A) = conv(vert{B) Q vert{{A) Q )) 

where conv(X) is the convex hull of X [14], vert(X) is the set of vertices of the polyhedron X y 
X Q Y ~ {x — y \ x E Xandy £ Y} and (X)o means the -polyhedron X in its initial 
configurations, where rvx is at the origin. This result and the existence of 0(n log n) convex hull 
algorithms for finite sets of points in K 3 [41], lead directly to an 0(v 2 log v) algorithm for CO^ z (B), 
where v = |vcrt(i4)| + |«ert(fi)|.Thc result also holds when A and B are convex polygons, but more 
efficient algorithms exist for this case. In particular, an 0(v) algorithm for CO% J (B) is described in 
[27]. 

3. 3. Variable Orientation of A 

When A is a three-dimensional solid which is allowed to rotate, CO A (B) is a complicated curved 
object in a six-dimensional Cspace A . Rather than compute these objects directly, the approach taken 
here is to use a sequence of two- and three-dimensional objects to approximate the high-dimensional 
CspaceA obstacles. In particular, a six-dimensional CspaceA obstacle for a rigid solid can be ap- 
proximated by projections of its (x } y, 2)-slices. A j -slice of an object C £ 5R n is defined to be: 

UPu...,Pn)£C\ li <P i <i i } 

Where u and 7^ are the lower and upper bounds of the slice, respectively. Then, if K is a set of 
indices between 1 and n, a /{'-slice is the intersection of all the j slices for j 6 K. Notice that 
a /sT-slicc of C is an object of the same dimension as C. Slices can then be projected onto those 
coordinates not in K to obtain objects of lower dimension. 

Figure 3 shows a two-dimensional example of slice projection. The objects shown shaded repre- 
sent the [x, y) projection of three 0-sliccs of CO A {B) when" 4 and B arc convex polygons. These slices 
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Figure 3. Slice projections of Cspaa A obstacles computed using the (x t t/)-area swept out by A over a 
range of values. Hach of the shaded obstacles is the (x, y) -projection of a 0-slicc of CO t 4 (■/?). Hie figure 
also shows a polygonal approximation to the slice projection and the polygonal approximation to the swept 
volume from which it derives. 



represent configurations where A overlaps B For some orientation of A in the specified range of 0. 
In [27] is a proof that these slice projections arc equivalent to the CO xy of. the area (volume) swept 
out by A over the range of orientations of the slice. Note that approximating the swept volume as a 
polyhedron leads to a polyhedral approximation for the projected slices of the Cspace^ obstacles, as 
shown in Figure 3. 



Slice projection has two important properties: 

I. A solution to a Findspace problem in any of the slices is a solution to the original problem, 
but since the slices are an approximation to the Cspace A obstacle, the converse is not neces- 
sarily true. 
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Figure 4. An illustration of the Findpath algorithm using slice projection described by Lozano- Perez and 
Wesley in [28]. A number of slice projections of the G space obstacles are constructed for different ranges 
of orientations of A The problem of planning safe paths in the high-dimensional Gspace A is decomposed 
into (I) planning safe paths via GO vertices within each slice projection and (2) moving between slices, at 
configurations that are safe in both slices. A\ represents A in its initial configuration, A3 represents A in its 
final configuration and A 2 is a simple polyhedral approximation to the swept volume of A between its initial 
and final orientation. 



2. The slice projection of a Cspace A obstacle can be computed, by using the swept volume 
operation, without having to compute the high-dimensional Cspace A obstacle, see Section 5. 



When rotations of A are allowed, the slice projection operation can be used to extend the Vgraph 
algorithm described earlier to find safe (but sub-optimal) paths [28]. A number of slice projections 
of the CspaceA obstacles are constructed for different ranges of orientations of A. The problem of 
planning safe paths in the high-dimensional Cspace A is decomposed into: 

1. planning safe paths via the vertices of Cspace A obstacles within each slice projection, and 

1 moving between slices, at configurations that are safe in both slices. 
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Figure 5. Models of objects are structured as trees of convex polyhedra: internal nodes represent the union 
of their sub-trees. Linked polyhedra arc used to represent manipulators; internal nodes represent joints and 
the leaves represent links. 'ITie nesting of sub-trees in the models of linked polyhedra reflect the cascading 
cITcct of joint motions. 



Both of these types of motions can be modelled as links in the Vgraph, therefore the complete algo- 
rithm can be formulated as a graph search problem. This approach is illustrated in Figure 4. However, 
since the obstacles arc three-dimensional, the Vgraph algorithm is subject to .'the drawbacks described 
earlier. 



4. Findpath for Cartesian Manipulators 

This section overviews an implementation 4 of the Findpath algorithm, for cartesian manipulators 
(sec definition below). Sections 5 through 8 present a more detailed description of the implementation. 
The system inputs are: 



4 The current implementation is written in LISP for the MIT LISP Machines. 
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Figure 6. A schematic representation of the link arrangement in two types of existing cartesian manipulators. 



1. A polyhedral model of the workspace — where each object is represented by a tree of 
convex polyhedra, see Figure 5(a). 

2. A polyhedral model of the manipulator — represented as a set of link bodies connected by 
rotary or prismatic joints, sec Figure 5(b). 

3. A kinematic model of the manipulator — currently, partly embedded in procedures which 
apply to the polyhedral model and partly in the model structure. 

4. A start and a goal configuration for the manipulator. 

The system output is a safe path from the start to the goal configurations of the manipulator. The 
paths are composed of a sequence of linear segments in the C space of the manipulator. 

The implementation described here is limited to cartesian manipulators, Lc. those having three 
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perpendicular translational degrees of freedom corresponding to the x, y and z axes and up to three 
rotary degrees of freedom, usually centered at the wrist. Figure 6 illustrates two different types of 
cartesian manipulators. The restriction to cartesian manipulators allows die use of the CO^ z {B) 
algorithm described in Section 3.2 as the main tool for capturing path constraints. 
The Findpath algorithm carries out the following steps in turn: 

1. Constructing the Cspace A obstacles — The slice projections of die C space A obstacles 
approximate the constraints on the configurations of the manipulator due to the presence of 
objects in the manipulator's workspace, see Section 5. 

2. Representing free space — Once the Cspace A obstacles are known, the system computes 
a decomposition of the space outside these obstacles into convex polyhedral cells; these 
polyhcdra are then linked into a graph structure called the Free Space Graph. Each node of the 
graph represents a free space cell and a link between cells indicates that they touch or overlap, 
sec Section 6. 

3. Searching for a safe path — The Free Space Graph is searched to locate a cell path, a 
connected set of free space cells that join the origin and the destination. From die cell path, the 
system derives a line path, a picccwisc linear path in the manipulator's C space, see Section 7. 

5. Computing the Cspace A Obstacles 

The first and most important step in the Findpath algorithm is that of computing die Cspace A 
obstacles arising from the presence of objects in die workspace. The C space A currently used by the 
system is the seven dimensional joint space of the manipulator, i.e. x, y, and z displacements, the 
three wrist rotations and the finger opening. The Cspace A obstacles are complicated objects in this 
high-dimension space. To avoid having "to deal directly with these objects, die system makes use of 
slice projection to approximate the Cspace A obstacles by a set of three-dimensional obstacles. 

The CO% JZ (B) algorithm of Section 3.2 computes an [x } y,z) cross-section of CO A {B) for a 
specified orientation of A. But, diis algorithm can be adapted to compute die (x, y,^)-slicc projections 
of CO A (B). The construct that i elates slice projections to the cross-sections is the swept volume of 
an object. The swept volume of A is the union of {A) a , i.e. A in configuration a, for a within the 
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configuration range denoted by [c,c']/<, where c and d are configurations of A and K is a subset of 
the configuration parameters. A configuration a is in the range [c, c% if, for each i in K, the i th 
parameter of a is between the i ih parameters of c and c'. For example, if c and c 7 are of the form 
(/3i,#2, A) and # = {3}, tlicn the swept volume of 4 over the range [c,<f\K refers to the union of 
A over a set of configurations differing only on /3j. The swept volume of A over this configuration 
range is denoted A[c } c']/ v . It can be shown [27] that the (x, y, 2)-slice projection of CO A (B) over the 
orientation range contained in [c, d\ K is the same as COJM*^ (B). 

In summary, the computational requirements of the slice projection technique are: 

1. Choosing a decomposition of the orientation ranges of Uic cartesian manipulator into sub- 
ranges, [c, cf] Ki to be used for slice projection. 

2. Computing polyhedral approximations to A[c, d\x for each orientation range. 

3. Computing CO T $l«{Bj) for each obstacle Bj and each orientation range. 

This section addresses these issues. First we assume that the orientation ranges defining Uic slices are 
given; the discussion of choosing slice parameters will be taken up at the end of the section. 

5. /. Computing the Swept Volume for Linked Polyhedra 

The swept volume of a polyhedron A over a range of translations is another polyhedron. Let 
T C / be the set of configuration parameters corresponding to the translations of A. If A is a convex 
polyhedron and the range of positions of the reference vertex of A over the range of translations 
[c, c'] K can be represented as a convex polyhedron V, then A[c, <f\ T = A © V where X © Y = 
{ x -f- y | x G X and y G Y}. Since A V = conv(vert(A) vert(V)), this leads to a direct 
algoridim for commuting the swept volume for translation. If the range of configurations includes 
rotations then the swept volume is not a polyhedron. In the rest of the paper it is assumed that 
a polyhedral approximation to the swept volume is always available. The Appendix describes an 
algorithm to compute a simple approximation to the swept volume of a convex polyhedron under 
pure rotation. 

The swept volume of A, a rigid object, resembles another rigid object with the same number 
of degrees of freedom. But for manipulators, modelled as linked polyhedra, the situation is more 
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Figure 7. Changes in the second joint angle from 2 to 0' 2 causes changes in the configurations of both link 
A% and link A 3. 



complex. Linked polyhedra are kinematic chains with polyhedral links and prismatic or rotary joints. 
The relative position and orientation of adjacent links, A { and A i+ i, is determined by die i ih joint 
parameter (angle) [36]. The set of joint parameters of a linked polyhedron completely specifies the 
position and orientation of all the links. 

Note that for a linked polyhedron, the position of link j typically depends on the positions of 
links k < j, which are closer to the base than link j. LctK = {j}, c == (<?;), d = {0% and [<:,(/]/< 
define a range of configurations differing on the j th Cspace { parameter. Since joint j varies over a 
range of values, links/ > j will move over a range of positions which depend on the values of c and 
c', as shown in Figure 7. The union of each of the link volumes over its specified range of positions is 
the 'swept volume of the linked polyhedron. The swept volume of links j through n can be taken as 
defining a new j ih link. The first j — I links and die new j ih link define a new manipulator whose 
configuration can be described by the first j — 1 joint parameters. On die other hand, the shape of the 
new link j depends not only on die /^-parameters of c and (/, i.e. 6j and f j> but also on 0\ for I > j. 
This implicit dependence on parameters of c and d that are not in K is undesirable, since it means 
Uiat the shape of the new j th link will vary. Letting K — {j , . . . , n}, then die shape of the swept 
volume depends only on the /^-parameters of c and d, while its configuration is determined by the 
(I — /^-parameters. A swept volume that satisfies this property is called displaceable. 
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Figure 8. Computing the swept volume for linked polyhedra. If lc,c r \ f< involves ranges of configurations of 
the second and third link, first compute the swept volume for the third link and then the swept volume for 
the union of the second link and the swept volume of the third link. 
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Given an operation for computing (a polyhedral approximation to) the swept volume of a 
polyhedron, see Appendix, then this operation is applied to computing die swept volume of linked 
polyhedra. The swept volume A [c, </]/< is computed by the following process, illustrated in Figure 8: 

1. Let t = n, where n > is the number of links in die linked polyhedra, |/| = n; let 

*• = •: 

2. Placed in configuration c; 

3. LetA* = A*UAii 

4. If i G K then let A* = A*[c t <?]{{}, i.e. update A* to be the swept volume of A* over the 
range of i ih joint; 

5. Let i = % — 1. If i = then stop, else go to step 3. 
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The swept volume obtained in this fashion can then be used to compute the CO^^JJBj). 

5.2. Computing slice projections for Cspace A obstacles 

If /i[c,c/]/v overlaps some obstacle B then, for some configuration a in the range [c, </]/<-, (A) a 
overlaps B. The converse is also true. If A[c, c f ] K is displaccablc, dicn CO A \ c ^ K {B) is the set of 
/ — K projections of those configurations of A within [c, d\x for which A overlaps B. Rquivalcntly, 
CO A [ CiC /] K (B) is die J — K projection of die [c, d\ K slice of CO A (B). HA is a cartesian manipulator 
and K is the index set for the wrist rotations of the manipulator, then the configurations of the swept 
volume are die (x, y,z) positions of some point on the manipulator. The algorithm of section 3.2 can 
be used to compute 00^^(3) and thereby compute die required slice projections of CO A (B). 

Given the swept volume of die manipulator model for a particular range of parameters [c, cf]^, 
the next step is to compute the slices of all the C 'space obstacles for die manipulator over that range 
of configurations; this set is denoted COS[c, c% In previous discussions of the CO% JZ (B) algorithm 
we have assumed that A and B were single polyhcdra; we saw in the previous sections -that both 
the object and manipulator models are structured as part trees, whose leaves arc convex polyhcdra. 
The actual model of a manipulator or a part is die union of die fringe, i.e. the set of leaves, of the 
corresponding part tree. Thus if A = Ui— i^t and J5 = U*Li#j* t,lc following result can be used 
in computing CO x ^ z {B): 

k A k B 

CO A {B)= (J IJcO^Bj), 

i=l i==l 

This result means that k A X /^applications of die CO^{B 3 ) algorithm must be carried out to 
compute CO^ yz (B) exactly. In die "pick and place" application, an exact model of all the C 'space 
obstacles is not usually needed since the manipulator will not move close enough to all the obstacles. 

The amount of time needed to compute the COS can be reduced by simplifying the geometric 
models of both the A{ and die Bj when appropriate. The current implementation uses a simple family 
of succcsivcly finer approximations for objects based on the part tree. Consider the part tree for 
an object Bj, where each of the leaves of the tree is a convex polyhedron. Define a covering node 
&>/ recursively to be eitlicr (1) the set containing just the root of the part tree or (2) obtained from 
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another covering node set by replacing some node, internal to the part tree, with all its descendants. If 
each internal node represents die union of all its descendants, then every covering node set is a com- 
plete model of the object. In practice, internal nodes of the part tree store the bounding rectangular 
solid 5 of the union of all its descendants instead of the union itself. Thus, the family of covering 
node sets represents progressively more detailed models of die part [29]. Using these approximations 
reduces the number of applications of CO^ z (Bj) needed to compute the COS, since the number 
of polyhedra in a covering node set is less than or equal to that in the full fringe. In addition, it 
can be used to simplify many of the individual computations, because when A and B arc bounding 
rectangular solids, computing CO^ JZ (B) is trivial. In particular, if tlic bounding solids arc represented 
by die endpoints of their main diagonal, e.g. A = (aj,a 2 ) and B — (61,62), then CO^ yz (B) =■ 
(61 — (a 2 — ai),&2). 

For simplicity, die current implementation uses a diree level part tree for the swept volume of 
the manipulator and for the objects in the workspace. Rich tree has a root node which models the 
complete object by one bounding rectangular solid. The descendants of the root are bounding rectan- 
gular solids for each of die convex components of the model and die leaves of the tree arc the convex 
polyhedra whose union is the complete object model. Therefore if the object is modelled as the union 
of k convex polyhedra, the part tree has 2k -f 1 nodes. Using this representation, CO% JZ (B) can be 
modelled as a tree of similar structure with 2(k A X k^) -f 1 nodes. Any covering node set of Uiis 
tree is an approximation to the Cspace A obstacle corresponding to B. In practice, the complete tree is 
not computed at once, rather the simplest approximation, the bounding rectangular solid of the whole 
object, is computed and successive covering node sets are computed as needed. This is discussed 
farther in section 6. 



5.3. Choosing the Slice Parameters 

So far we have assumed that the configuration ranges defining the Cspace A slices were given as 
input; in this section, die choice of ranges is discussed. The primary choice is how large to make the 
ranges, since it is this that affects the system's capability to use changes in die orientation of die hand 



5 A bounding rectangular solid for a polyhedron is a rectangular solid whose edges arc parallel to the coordinate axes 
and that completely includes the polyhedron. 
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to avoid obstacles. In particular: 

1. The larger the orientation range of a slice, the larger the manipulator's swept volume, the 
larger (and less accurate) the Cspace A obstacles and the fewer the legal configurations and legal 
motions of the manipulator. 

1 The smaller the orientation range of slices, the larger the number of slices needed to cover 
the C space and the more time needed to compute the COS and to search them for a path. 

These conflicting effects can be balanced by taking advantage of the fact that, for "pick and place" 
motions, the accuracy requirements are higher near die start and the goal of the path, where the 
manipulator is moving near obstacles, than along the rest of the path [28] [48] [49]. This suggests 
defining slices with small rotation ranges centered around the orientations of the start and the goal; 
slices with larger ranges may be used for the remaining orientations. This approach is used in the 
current implementation. In particular, a COS is defined for die orientation of the manipulator in 
the start configuration and one for the orientation manipulator in the goal configuration; these COS 
correspond to slices with singular orientation ranges, i.e. where the upper bound of the range equals 
the lower bound 6 . In addition, the total range of parameters in Cspace A is divided among some 
number of other slices 7 each with non-singular ranges. Furthermore, slices with singular ranges are 
defined for configurations at die intersection of the slice parameters of die "larger" slices. This last 
type of slice allows moving between safe configurations in the "larger" slices. 

Note diat the computational burden of adding an extra slice is very low if bounding rectangles 
arc used for objects. This sacrifices some of die potential maneuvering space, but gains a very large 
increase in speed. This is the compromise taken in the current implementation. 

Motions within a slice with a singular orientation range are limited to translations, while rotation 
is legal within a slice with non-singular ranges. Therefore, the classes of motions allowed by the 
system arc those composed of translations interspersed with rotations, but where Uic rotations happen 
in increments defined by the slices parameters. lTiis means that this approach may fail to find a safe 
padi in situations where: 



6 A slice with a singular range is the same as a cross- section 
7 Currcntly varying between 8 and 64. 
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1. all safe paths require rotations combined with translations at a finer resolution than that 
allowed by the slice ranges, and/or 

2, the orientation ranges chosen, although adequate in size, do no match those required in the 
problem. 

These problems can be reduced, at the expense of more computation, by using more slices with 
smaller ranges. But, there exists problems which require continuous rotation along a path. In practice, 
most robotics applications do not use the very crowded environments that require very high rotation 
resolution for the "pick and place" motions. The reason for this is that safe paths in such environ- 
ments are very hard for humans to specify, are subject to positioning errors of die parts and are 
difficult for most industrial robots to execute reliably at medium or high speeds. 

6. Path Searching and Free Space 

Having computed the Cspace.\ obstacles, it still remains for die system to find a path among these 
obstacles. This section briefly touches on alternative strategics for finding safe paths. 

One approach to finding paths among obstacles is to search for die shortest path between the start 
and the goal, without considering other constraints. For example, the Vgraph algorithm described in 
Section 3 follows tliis approach. But, the approach has some important drawbacks. Shortest paths in 
Cspace A move along the boundaries of the Cspace A obstacles and are, therefore, very susceptible 
to model inaccuracy and position error. This problem can be alleviated by adding a uniform "safety 
margin" around the obstacles, but doing so might disqualify some feasible paths. Furthermore, 
no efficient algoridims currently exist for finding optimal paths among three-dimensional obstacles. 
Unlike the situation in two dimensions, there is no finite set of points through which shortest paths 
arc guaranteed to pass. Thus, algorithms would have to be based on iterative numerical methods. For 
these reasons, only heuristic algorithms for finding safe paths will be considered here. These heuristic 
algorithms require less execution time and can be extended to consider criteria such as safety margins, 
but they will not find the shortest path, 

Another issue is whether die padi search is conducted using primarily a representation of the 
Cspace A obstacles themselves, as does the Vgraph algoridim, or of the free space outside the 
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obstacles, as in [48] [49]. 'Although these representations arc equivalent, they lead to different heuristic 
algorithms. The current implementation uses the free space style of algorithm because it simplifies 
the formulation of different search heuristics, e.g. the use of variable resolution space representations 
described below. 

The remainder of the section deals with the free space representation technique employed in the 
Findpath implementation. Section 7 discusses the path search algorithm used on this representation. 

6 J. A Free Space Representation 

The basic goals for a space representation are accuracy, speed and compactness. In addition, 
it should facilitate heuristics for the task at hand. Hie most important heuristic for a space repre- 
sentation is to avoid excess detail (and therefore time spent) on parts of the space which do not affect 
the operation. Therefore, the space representation should not have to maintain a perfectly detailed 
model everywhere. Instead, it should have the capability of maintaining a rough model and be able to 
selectively refine [48] [49] subsections to be as detailed as necessary. 

A number of proposals exist for representations of space and objects in space [9] [25] [42]; most 
of these divide the space into a set of cells. The proposals can be partially characterized along the 
following dimensions: 

1. Shape uniformity — are all cells equally shaped? 

2. Size uniformity — arc all cells the same size? 

3. Orientation uniformity — are all cells oriented uniformly? 

4. Ordering principle — are the cells ordered into an array, multi-list, tree, or graph. 

We will not consider representations which use cells of uniform shape and/or size, since they typically 
require large numbers of cells to achieve sufficient accuracy 8 . Instead, wc use a hybrid cell repre- 
sentation employing two types of cells: (1) rectangular solids aligned with the axes and (2) arbitrary 
convex polyhedra. The idea is to use the simple rectangular cells away from obstacles where repre- 
sentation economy is important and polyhedral cells where high accuracy, e.g. near an obstacle, is 

8 Udupa [48] [49] employed a free space representation which used rectangular cells of variable size. This approach is 
adequate for motions that do not closely approach the obstacles. 
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Figure 9. This figure illustrates, in two dimensions, the space representation employed in the implementation 
of the Findpath algorithm, (a) A sample Csjxice A obstacle with its part representation, (b) The resulting 
space representation. Rectangular nodes indicate mixed cells, round nodes indicate full cells, and triangular 
nodes indicate empty cells. 



needed. 



The space representation described below is analogous to the part representation described 
earlier, except that a new type of node is introduced. The part tree representation uses rectangular 
bounding cells as internal nodes and polyhedral cells as leaves. The leaves represent space that is 
FULL, i.e. completely occupied by an object. The internal cells represent MIXED space, i.e. cells 
which are part FULL, part KMPTY. But, note that the part tree does not have an explicit repre- 
sentation of the EMPTY space. The space representation simply adds explicit EMPTY cells to the 
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parts tree representation. Then each- internal MIXHD node becomes the union of its descendants. 
In addition, the space representation introduces a new MIXED root node from which all the part 
representations descend. 

The space representation is built up starting with a bounding rectangular solid representing the 
workspace, this is the first MIXED cell. The descendants of tliis node are the MIXED cells cor- 
responding to the roots of the trees representing each of the CO% JZ (Bj), as described in Section 5.2 
and a set of EMPTY bounding rectangular solids representing die free space outside the MIXED 
cells. The representation of each MIXED cell can be further expanded into other EMPTY, MIXED 
and FULL cells, culminating in a representation involving only EMPTY and FULL convex polyhedral 
cells as leaves of the tree and MIXED cells as internal nodes, Figure 9. The polyhedral representation 
of each EMPTY cell must be computed so that it does not overlap any MIXED or FULL cells. As 
with the part representation, any covering node set of this tree represents a complete model of the 
space, at some non-uniform resolution. This hybrid cell representation is based on a generalization 
of the quad tree representation used for images [8] [17] [18] [20] [43] and the oct-tree representation of 
objects [3]. 

The operations on the space representation described above arc very efficient when dealing with 
bounding rectangular solids. The most expensive operation is when the volume difference of a 
MIXED rectangular cell and a FULL polyhedral cells must be computed 9 ; this operation results in 
a description of die EMPTY cells. However, this need only be done when high accuracy is required, 
usually near the start and the goal of the path. Therefore, die representation meets the criteria stated 
at the beginning of the section. 



6,2. Building a Free Space Graph 

The process described in Section 5 produces a slice for each C 'space A obstacle over each of the 
orientation ranges, [c t , c/J^, of the manipulator's wrist. The set of slices for all obstacles over one 
orientation range is denoted COS^c'^. For each of these COSu a space representation is com- 
puted, SR{ 9 as described above. For each of these SRi, a Free Space Graph is built, FSGi, this is 
a graph where each node is an EMPTY cell in the SR( and a link indicates that the cells touch or 
^fhc current implementation of this operation uses repeated applications of a cutting and capping operation [6]. 
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overlap 10 . In addition, it is necessary to add links to each FSGi that connect to nodes of other FSGj 
whose rotation range overlaps that of FSG it That is, for EMPTY cells C x £ SRi and Cj £ SRj, if 
there is some configuration c contained in both cells, then links must be placed between C t - and Cj. 
This is so because the existence of c guarantees that it is possible to pass from any configuration in Q 
to any in Cj and viceversa while remaining outside all the obstacles in COSi and COSj. The resulting 
composite FSG is then searched for a path, since each path through the graph corresponds to a class 
of safe paths in Cspace A and vice-versa. 



7. Path Searching 

The Findpath problem is to find a path between two points, the start and the goal, while staying in 
the free space. In the current implementation, this is carried out by the following steps: 

1. Choose die largest EMPTY cell in any of the SRi enclosing the start configuration. 
Otherwise, choose some MIXED cell containing the start and expand the representation of 
this MIXED cell into its constituent EMPTY, MIXED and/or FUEL cells. If an EMFIT cell 
contains the start configuration, stop, else repeat. Note dial this computes successively finer 
models, i.e. successive covering node sets, of the specific area around the start without having 
to expand the complete model or even any complete part tree. If no EMPTY cell is ever found, 
the task is impossible since the start configuration causes a collision. 

2. Perform step 1 for the goal configuration. 

3. Construct a Free Space Graph as described in Section 6.2. At this point, the Free Space 
Graph is in its final form; the current implementation does not refine the space representation 
further. 



£**»> 



10 The current representation allows EMPTY cells to overlap each other but not MIXED or FULL cells. 
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4. Search for die shortest path in the Free Space Graph from the cell including the start to 
that including the goal. r Hie graph search operation can be carried out by any of the standard 
shortest path algorithms [13]; the current implementation uses the A* algorithm [15]. These 
shortest path algorithms require that a weight be assigned to each of the links of the Free Space 
Graph e.g. indicating the time required to traverse the cells. How this may be done is discussed 
below. If no path exists, this may be due to the approximations and quantizations used in the 
solution, see Section 7.3. 

5. Choose a line path contained in die cell path. This problem is discussed in Section 7.2. 



7./. Assigning Link Weights for the FSG 

The definition of an "optimal" path, or even a "good" path, assumes some choice of performance 
index. The current implementation uses estimated time of travel along the path as die index. If 
Capace A is the manipulator's joint space, then the time to travel between two configurations can be 
estimated as the maximum time for any of the joints to travel, at the maximum rated joint velocity, 
between the joint settings at each configuration. The weights assigned to die links in the FSG should 
therefore reflect the time needed to travel between two overlapping cells along the optimal path. Of 
course, no weight assignment can actually do this since it requires knowing the complete optimal path. 

A simple alternative is to assign to a link die estimated time of travel between die centroids of the 
cells that it connects. This weighting function has the advantage of being very easy to compute. For 
small cells it provides a good approximation of the actual time to traverse the cells, but for larger cells 
it might overestimate or underestimate the actual time, see Figure 10. The current implementation 
uses the ccntroid weighting function, but docs not divide the large EMPTY rectangular cells into 
smaller cells; this will be implemented in the near future. 

A HWfc eottiplex weighting function, which would typically produce faster paths, is the following: 
The weight on the link between cell C and C" is assigned the time to traverse C from p, the point 
of entry to C, to p', die point of entry into C". The point p' is the one on C D C" diat minimizes 
die distance 11 to the line between p and the goal. The initial C is die cell that contains the start 
1 ! Actually, the difference in time between the straight line path and one going through this point. 
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Kieurc 10 Two dimensional illustration of failings of (he ccniroid weighting ^^J^S^SSf^ 
would use to evaluate the distance between cells. 



configuration and the initial p is the start configuration. Clearly, this technique requires much more 
computation that the ccntroid weighting described above. For most applications, the simpler centroid 



function, together with cell splitting should suffice. 



7.2. Choosing a Line Path 

The search of the FSG produces a list of EMPTY Capace A cells that touch or overlap; it is still 
necessary to choose a specific path, i.c. some curve, within these cells. The simplest type of path 
to choose is a piccewisc linear one, although Die cells simply place configuration constraints on the 
manipulator along the path and any path satisfying those constraints will be safe. 

If the centroid weighting has been used for the links, it is natural to choose a piccewisc linear 
path that traverses the centroids of the cells. Of course, the straight line path between two ccntroids is 
not guaranteed to remain within the cells and might therefore not be safe. Therefore an intermediate 
configuration in the intersection between adjacent cells should be chosen. The ccntroid of the inter- 
section of adjacent cells on the path can be used for this purpose; this is the technique used in the 
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current implementation. Alternatively, this point could be chosen so as to minimize the deviation 
from a straight line path between the centroids. If the cell size is small enough, such paths are 
adequate for most tasks. 

The more complex weighting scheme described earlier produces a sequence of entry points into 
the cells which may be connected directly to obtain a path. Since the points arc contained in the 
intersection of the cells, a straight line connecting diem is guarantcd to be in the cell. 



7.3. Dealing with Path Search Failure 

If the path search algoridim fails to find a safe path, the reason for failure could be one of the 
following: 

1. No safe paths exist. 

1 No safe paths exist at die quantization of orientations chosen. 

3. The approximations of objects by bounding rectangular solids has removed necessary 
maneuvering space. 

The last two causes of failure may be overcome by decreasing die orientation quantization and/or 
increasing the representation detail in the space representation, both at the expense of extra computa- 
tion. This suggests the possibility of increasing the accuracy of the space representation when a path 
search failure ocurrs. The current implementation does not exploit diis possibility. 



8. Examples 

This section presents output from the implementation running on a simple example. The results 
are collected in Figure 11. 

a. The initial and final configuration of the model, including the manipulator model. Note 
that the manipulator must rotate to execute this motion. 
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Figure 11a . , 

The start and goal configurations and the world model 
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Figure lib 

START COS: The Cspace obstacles for the manipulator in the 
start configuration. 
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• Figure lie 

GOAL COS: The Cspace obstacles for the manipulator in the 
goal configuration. 
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Figure lid 

The Cspace obstacles for the swept volume of the manipulator 
over a range of configurations of the wrist. 



216 




^-. 



Q 



Figure lie 
The Cell Path with superimposed Line Path 
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Figure llf 
The Cell Path and Line Path superimposed on GOAL COS 
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h. The COS for the start configuration. Kach convex solid in. (he figure is a representation of 
CO x ^{Bj) t Note that most of these Cspace A obstacles are rectangular solids, except for those 
arising from die interaction of the hand, A*, with block B { and the fingers, A { and A 2 , with 
the table. In these cases, the manipulator is so close to these obstacles that its configuration is 
inside the bounding rectangular solid for the configuration obstacles (In practice, die sides of 
the bounding rectangular solid arc displaced outward by some small e). This condition causes a 
detailed expansion to be carried out. 

c. The COS for the goal configuration. In the goal configuration none of the obstacles needs 
to be expanded in detail. 

d. The COS for one of the intermediate configuration ranges. This COS is defined for the 
manipulator's swept volume over a range of orientations of the wrist and hand. One bounding 
rectangular solid, A\, approximates the swept volume of the hand and fingers, A\ U A% U A3. 
The solids A4 and As remain unchanged. 

e. The cell padi and die line path. This shows the cells from the various space representations 
that compose the cell path. One group of cells correspond to free space for the initial 
configuration, one large cell comes from the intermediate configuration (where the hand rota- 
tion takes place), and the last group of cells correspond to the final configuration. The line 
path shown goes dirough the centroid of each of the cells and also through the centroids of 
die intersection of adjacent cells on the path. Notice that because the cells are large, this path 
strategy produces paths that move too far from die obstacles. This could be overcome by sub- 
dividing the cells before finding die line path. 

f. The cell path superimposed on the start COS. This shows die relative placing of the free 
cells relative to the obstacles. 



9. Choosing Grasp Configurations 

The preceding sections have discussed die problem of finding safe paths for the manipulator; this 
is only part of the "pick and place" synthesis problem. The major remaining problem is choosing 
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a grasp configuration on the part, P. For simple parts and non -cluttered environments, grasping is 
amenable to simple ad-hoc solutions. As a step in die solution of this problem, we deal here with 
choosing grasping configurations for relatively simple parts in cluttered environments. In this section, 
a C space approach to this problem is described, although no implementation of this approach to 
grasping currently exists. 

The grasping problem is related to tine Findspace problem introduced in Section 3, insofar as it 
involves choosing a safe configuration among a set of obstacles. But, there arc additional constraints 
on the choice, for example: 

1 . the manipulator's fingers must be in contact with P, 

2. the configuration must be reachable, and 

3. P must be stable in the manipulator's hand, i.e. it will not slip in die hand during a motion. 

The first two conditions, contact and reachability, reflect additional geometric constraints on the solu- 
tion to the Findspace problem. The third condition, stability, reflects aspects of grasping beyond the 
purely geometric. Stability will be briefly discussed later in the section. 

The approach to grasping described here is based on die one described in [25] and [26]. The 
basic idea is to build an explicit description of the set of configurations of the manipulator A for 
which the inside of the manipulator's fingers are in contact with specified surfaces of P. This set 
of configurations is some subset of CO A {P), call it G. Feasible grasp configurations are those in 
G, that do not cause any collisions with other objects in the workspace, i.e. that are outside all of 
the CO A {Bj). In this, section, the details of this approach are discussed. We make the following 
simplifying assumptions: 

1. The manipulator is cartesian and its hand is a parallel jaw, i.e. two parallel fingers that move 
along dicir common normal. 

2. Only parallel planar surfaces, whose distance from each other is less than the maximum 
finger opening, arc candidates for grasping. These are known as grasp surfaces. 

These assumptions simplify the method for identifying feasible grasp configurations, while suggesting 
its usefulness and providing the foundation for a more general approach. 
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Figure 12. The definitions of Pi, Pj % Ft, F2, and // used in choosing grasping configurations. 
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9.1. Feasible Grasp Configurations 

Let Pi and Pj be the parallel faces 12 of P to be grasped, and F± and F 2 be the inside faces of 
the manipulator's fingers, Figure 12. Under the two assumptions stated above, when A grasps P, Fj 
and F2 are coplanar with P{ and Pj respectively. Under these conditions, the legal (x, y, z) positions 
of tva are restricted to some plane H diat is parallel to Pi and Pj. Let Ga{Pu Pj) be the set of 
configurations of A for which tva is in H and for which P t , Pj, Fi, and F2 are mutually parallel. Note 
that Ga{Pu Pj) represents those positions and orientations where A could be when grasping P t - and 
Pj, without specifying the distance between the fingers. G A {Pu Pj) is called the grasp set for P x * and 

Note that not all the configurations in Ga{Pu Pj) are feasible grasp configurations, cither because 

12 Note that objects in the current implementation are modelled as unions of convex polyhcdra. Convex polyhedra are 
defined as the intersection of a finite number of half-spaces, where each half-space is bounded by a plane. The portion 
of each bounding plane on the boundary of the polyhedron is a convex polygon, known as a face of the object 
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the fingers are not in contact with the grasp surfaces or because the manipulator configuration causes a 
collision with some other object. Therefore, we must impose two additional restrictions: 

1. The internal faces of the fingers must overlap the grasp surfaces. 

1 The manipulator must not collide with any other object in the workspace, i.e. theBy. 

With these restrictions on the configurations in the grasp set, we obtain the set of feasible grasp 
configurations, called a feasible grasp se/ and denoted FG A (P{ t P 3 ). 

Define the configurations of F { and F 2 to correspond to diose of die manipulator, i.e. each 
position and orientation of these faces is characterized by the manipulator configuration which would 
place diem there. From dicsc definitions it follows that CO Fl (Pi) is die set of those configurations of 
A for which theFi is in contact with P t . Furthermore, CO Fl (Pi)f\G A {Ph Pj) are Uiose configurations 
for which die finger is in surface-surface contact with Pi. Therefore, it follows that 

FG A (p i} Pj) = (co Fl (p { ) nco^Pj) n G A (p if Pj)) - \J co A (Bj) 

In diis definition, we must let P be one of the By, say B p , so as to avoid collisions with P while 
approaching a grasp configuration, but we must also allow A to contact P on die grasp surfaces. The 
answer is to add a slight displacement inward to P; and Pj, when computing CO A {B p \ while using 
the original definition in die computation of CO Fl (P() and CO F2 (Pj). 

The feasible grasp set, as defined above, is a volume in a six-dimensional Cspace A . We do not 
have algorithms for computing this volume exactly. The algorithms of Section 3 serve only to com- 
pute slice projections of die Cspace A obstacles. It is clear that die same must be done for the feasible 
grasp set, namely computing its slice projection for some range of orientations. Such a slice would be 
the setoff, y,z) positions of A that, for some range of orientations of A, are in contact with P, but 
outside all of the Bj. Presumably, diis requires using the slice projections of CO Fl (Pi), CO F2 (Pj), and 
the CO A (Bj). A problem arises when trying to do this, because slice projections were defined over 
simple orientation ranges of the cartesian manipulator's wrist defined in Section 5. These ranges are 
not, in general compatible with the ranges of orientations that define G A (Pi,Pj). For a position of 
rv A on H, only a small range of orientations will result in configurations that are in G A (P if Pj), yet for 
that position to be in a slice of FG A (P i} Pj) it must be the case that no orientation within the slice's 
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defining range causes a collision. Therefore, few, if any, configurations in the grasp set will be feasible 
grasp configurations. 

The solution to this problem is simply to define a new set of slices whose orientation ranges are 
subsets of the orientation ranges in G A (P i} Pj), Note that a configuration in such a slice already 
satisfies the orientation constraints of the grasp set. Therefore, only the position constraints, i.e. that 
the (x, y,z) position be in H, need to be enforced to obtain the intersection of a Cspace obstacle in 
that slice with the grasp set. This removes the need of computing die complete representation of the 
obstacles, while simultaneously avoiding the problems introduced by irrelevant orientations. 

Computing the obstacle slices for orientations in 'the grasp set requires being able to compute the 
swept volume of the manipulator over orientation ranges that arc not the simple ranges of joint angles 
defined in Section 5. Let R be the set of orientations in the grasp set that define a slice and denote 
the swept volume of A over/? as A[R], Algorithms for approximating the swept volume over these 
ranges can be based on the simple approach described in the Appendix. The important constraint on 
the approximation to A[R] is that it docs not intersect die grasp surfaces for positions of rv A on H. 

In addition to the manipulator displacing and rotating, die manipulator's fingers may move per- 
pendicular to the grasp surfaces. This additional degree of freedom has not been discussed above. In 
fact, it poses no additional problems; the motion of die fingers can be treated, via slice projection, 
uniformly with rotation. This simply requires including die space swept out by the fingers during 
closing, in die swept volume used to define slices of die COa(Bj). 



9.2. Overlap of Finger and Surface 

TTie approach described above deals adequately with the CO A (Bj) in the definition of feasible 
grasp set, but is less succcsfiil in dealing with COp x {Pi) and COp 2 {Pj). The reason for this is that 
a position in the slice projection of COp^Pi) simply indicates that for some orientation of A in the 
slice, the finger is in contact with P t . What is required instead is the set of positions which for nU 
orientations of A in the slice, there is contact. In fact, we would like to guarantee diat the area of 
contact between die fingers and the grasp faces always exceeds some fixed area. How diis may be 
accomplished is discussed below. 
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Figure 13. Defining the configurations of A for which F overlaps P. (a) Illustration of the definition of T k 
and S. (b) Illustration of CI%)>*(I\) Q S, with two positions of F k s reference vertex (indicated by the small 
circles) showing the area of overlap includes an area of the form T fe 0s, for some a G S. 



Let F k and Pi be, respectively, a finger surface and the corresponding grasp surface. We define 
T k to be a small strip at the tip of F k , such that F k = T k © S, where S is the set of points along a 
line segment, as shown in Figure 13. Again, we assume that the configurations of T k correspond to 
those Of F k (and therefore A). Assume A is in some configuration c £ G A {P ij P 3 ), so that F k and Pi 
are coplanar, then ClffiPj) is the set of {x, y, z) configurations of T ky and therefore of F k and A, 
for which F k Pi 2 T k . But, we do not want to restrict the overlap between F k and Pi to be at the 
fingertip; instead, we want the area of overlap to include some area T^, obtainable by translating T k 
along 5, i.e. T k — T k © {s}, with a E S. It is easy to show that 
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CIf k z {Pi) eS={c\3seS:PiC) {F k ) c D {T k ) c {a} } 

Therefore, this is the desired set of configurations, see Figure 13. This result can be applied to com- 
pute t|ie sjiccs needed for the feasible grasp set. If R is the orientation range defining the slice, then 
CI^VlJPi) S[R] represents die set of (x t y, z) configurations that, for orientations in P, guarantee 
that the contact between F k and Pi includes T k . Note that this approach can be generalized to any S 
and T k such that F k = T k S; as T k becomes smaller and approaches a point, then 5 approaches F fc . 

9.3, Safety at (he Destination 

So far, the definition of FG A {P u P i ) only embodies constraints relating to safety at Ps initial 
configuration, however a grasp configuration must also be safe at P's final configuration. Clearly, 
another feasible grasp set can be computed at Fs final configuration, say FG A {P f it P f 3 ) where the 
primed faces indicate the faces at their final configuration. But, these two feasible grasp sets cannot 
be intersected to obtain those grasp configurations that are safe for both configurations of P f - be- 
cause a grasp configuration corresponds to different manipulator configurations at each different 
configuration of P. What is needed is a way of defining those grasp configurations in Fs initial 
configuration that would lead to a collision when P is in its final configuration, Figure 14. 

A grasp configuration establishes a fixed relationship between the fingers and the grasped part, 
P. Let the final configuration of P be obtained by a displacemnct consisting of a translation t and a 
rotation r, indicated by D ttr {P). Clearly, any set of positions X bears the same relationship to A, r (P) 
asDf^X) bears to P. Therefore, if CO%' z {Bj) is a set of positions of A which cause collisions atP's 
final configuration, then D^ r ] {CO% yz {Bj)) represent infeasible grasp configurations, Figure 14. This 
result also holds for swept volumes oM, therefore it may be used to ensure safety at the destination in 
the definition of feasible grasp sets. 

9.4. Computing the Feasible Grasp Set 

The discussion in the preceding subsections is summarized in the following definition of feasible 
grasp set, for some range of orientation in the grasp set. We denote this orientation range as J2, and 
klR* denote the same orientation range as R relative to P, but at P's destination. We also let {t t r) be 
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Figure 14. (a) A side view of a manipulator hand, composed of a finger and a "palm", hold- 
ing P at the initial and final configuration, (b) In the initial configuration, the shaded area 
represents COpf(Pi) — {J CO x A yz {Bj), i.e. the feasible grasp configurations for 4, considering 
only safety at the origin and letting T be a point, (c) The CO% y *{Bj) for the final configuration 
of A and P. (d) The shaded area represents CO^Pi) - \Jj co a v *(Bj) V D^CO*?^))* 
which is the feasible grasp set that takes into account safety at the destination. 
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the displacement between the initial and final configurations of P. Ilien, the feasible grasp set, for the 
orientation range R and displacement (t , r), is: 

MflfaiPi, Pi) = {(CI?T M {Pi) O CIKfafTj)) Q S[R}) - |J CO^ t] (Bj) U D^CO^Bj)) 

i 

All of the elements in this definition can be computed using the CO xyz algorithm of Section 3.2 and a 
swept volume algorithm. 



9.5. Approach and Departure 

Configurations in the feasible grasp set, as defined above, are guaranteed to be safe both at P's 
initial and final configuration. While these conditions are sufficient in most situations, they do not 
guarantee that the feasible grasp configurations can be used during a "pick and place" operation. For 
a feasible grasp configuration to be a legal grasp configuration, it must allow the manipulator to reach 
and depart Fs initial and final configurations. Summarizing, the following conditions must hold for a 
legal grasp configuration: 

1. It must be possible to reach it from the initial configuration of the manipulator. 

2. It must be possible to remove P from its in ttial configuration safely. 

3. It must be possible to reach the P's final configuration with P held in the hand. 

4. It must be possible to withdraw the manipulator from P's final configuration. 

The Findpath algorithm described in the preceeding sections can be extended to deal with 
the problem of choosing a grasping configuration that is reachable from the manipulator's initial 
configuration. As we saw above, the feasible grasp configurations, over some range of orientations, are 
those within some specified volume of C8pace A , but outside the slice projections of suitably defined 
CspaceA obstacles. Hence, they arc equivalent to the slices, COS[c, cf\ of Section 6.2. Therefore, 
a free space representation for the feasible grasp configurations can be constructed and the resulting 
free cells linked in the Free Space Graph. The feasible grasp configurations for alternative grasp 
surfaces can also be linked into the graph. In the resulting FSO, any path from die cell containing 
the origin to a cell containing a feasible grasp configuration shows that this grasp configuration may 
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be reached from the origin. The path searching process must be modified to search Tor any cell 
which. contains a suitable grasp configuration, rather than searching for a particular cell containing the 
destination. 

Similarly, departure from the origin and approach to the destination could be handled by testing 
whether the destination is reachable, using the FSG constructed as above. The difference is that now 
die hand is holding P, therefore the polyhedral description of P must be treated as if it were part of 
die manipulator. This requires adding a new set of Cspace A obstacles, arising from the interaction of 
P and the objects in the workspace, to die ones already computed for the manipulator. This is entirely 
analogous to modifying the description of the manipulator, which is already modelled as a union of 
convex solids. But, the geometric relationships between P and die A { arc determined by the grasp 
configuration, which has several degrees of freedom. The problem can be approached by treating 
these additional degrees of freedom, via slice projection, just as the wrist rotations were treated. This 
approach imposes a great cost in additional computation. A simpler, though less general, technique is 
to use heuristics in choosing a feasible grasp configuration and dien test, via the path search process, 
whether that grasp configuration permits departure. If it does not, a new configuration might be 
chosen and the process repeated. This approach would be not be adequate for very cluttered environ- 
ments, or situations involving parts mating at die destination. In such environments an approach based 
on slice projection would also be susceptible to failure. Further research is needed in this area. 



9.6. Stability in Grasping 

We have thus far not considered the issue of stability of the feasible grasp point. An adequate 
treatment of stability in grasping is not yet available, although some promising approaches exist [5]. 
The techniques described in this section can be used to implement two simple grasping heuristics, 
which work adequately when (I) the manipulator hand is made up of rigid fingers, (2) die object to be 
grasped, P, is small relative to die manipulator hand and (3) parts mating effects arc ignored. The two 
heuristics are: 

1. Ensure at least a minimum contact area of the fingers with the grasp surfaces. The amount 
of overlap should depend on object properties such as weight and surface smoothness. 
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2. The perpendicular projection of F's center of mass should be near to Fi Pi and F<i f) Py 

The implementation of the contact area heuristic was discussed above, Section 9.2. The center 
of mass heuristic can be implemented by giving preference to grasp surfaces for which the center of 
mass, projected onto the plane containing P t -, falls within P{ and similarly for P 3 . Furthermore, for 
specified grasp surfaces, the choice among legal grasp configurations should minimize the distance of 
the prjection of the center of mass to the area of overlap between finger and grasp surface. 

These heuristics, though adequate for many tasks, are not a substitute for a general theory of 
stability in grasping. This remains one of the most interesting open problems in robotics, 

10, The Effect of Uncertainty 

In the preceeding sections we have assumed that: 

1. the configuration of all die objects is known exactly, and 

2. the configuration of the manipulator can be controlled exactly. 

Both of these assumptions are only approximations to reality. In practice, configurations can only 
be known to within some uncertainty. Both of these sources of uncertainty affect what manipulator 
motions are safe. 

10. L Modelling Worst-Case Uncertainty in Capoee^ 

In Cspace A , the two sources of uncertainty have similar effects, i.e. modifying the shape of the 
C f space a obstacles. This section deals with techniques for taking into account these effects. 'ITie 
following notation is useful in the discussion. Let e = (/%) = (/?j, . . . ,/? n ) G W 1 and similarly, let 
configurations be (ii) = (71, . . . , 7 n ) £ 5J n . The index set {1, . . . , n} will be referred to as /; let 
K C J. The set U K (e) denotes the set of configurations in Cspaee A whose /^-parameters arc less than 
the absolute value of the corresponding parameter of e. 
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Uncertainty in the configuration of A in C space A can be represented as a region around its 
nominal configuration, c; within this region are all the configurations that A may be in. Simple regions 
can be characterized by {c} U K {e A ). Assume that (^) ft B ^ 0, i.e that a £ CO A {B). Any 
nominal configuration a f such that o! -\- x = a, for x G Uk{ca), should also belong to CO A {B). This 
means that under uncertainty of A, CO A (B) should be replaced with CO A (B) Q U K (e A ). In practice, 
we do not ever compute CO A {B)\ rather, we compute slice projections of it using the swept volume 
of 4 over ranges of orientation parameters, R. Therefore, die orientation and translation uncertainty 
must be treated separately. Orientation uncertainty affects' the definition of the manipulator's swept 
volume. For example, to compute a slice with parameters [c, c']h, the swept volume A [c~~e A , c'-f e A ]ft 
is used in place of A[c t cf]n. The effect of the uncertainty in the translation parameters, T, can be 
computed as indicated in Section 5. 1 , using the CO% fZ (B) algorithm. 

The worst-case effect on CO A (B 3 ) of uncertainty in the configuration of the By, can be modelled 
by replacing B with the swept volume of B over the uncertainty range. Alternatively, if the uncer- 
tainty in the configuration of Bj can be approximated by an uncertainty in translation 13 , Ur(es 3 ) then 
the uncertainty of A and B can be combined into a single uncertainty 14 and treated as die uncertainty 
of A. If T is the set of indices for translation parameters, then the combined uncertainty is: 



UT^ A ) = V T {e Bo )eUr{e A ) 



10.2. The Effect < of Uncertainty on "Pick and Place" Synthesis 

The presence of uncertainty significantly affects manipulator programming in general and the 
synthesis of "pick and place" motions in particular. One approach to planning motions in the 
presence of uncertainty is to plan paths that are safe under the worst case uncertainty, i.e. paths 
outside the expanded Cspace A obstacles defined above. This approach rules out most operations 
that. Involve moving near objects, e.g. grasping. Another approach is to assume that uncertainty 
does not significantly affect the outcome of most operations and to plan motions assuming nominal 

13 This can be done by defining a new translation uncertainty such that the swept volume over this range of positions 
will contain the swept volume over the original uncertainty range. 

l4 This assumes that the translation space of die manipulator is the same as that of the objects in the workspace, which 
is true for cartesian manipulators. 
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configurations. A compromise position is to redefine the "pick and place" synthesis problem so as to 
isolate those operations that are most susceptible to uncertainty from those others where uncertainty 
plays a relatively minor role. The latter can be addressed by the techniques outlined in this paper, the 
former require a different approach. One possible re-definition of the "pick and place" problem is the 
following: 

1. Find a nominal grasp configuration assuming that there is no uncertainty. . 

2. Identify a grasp approach configuration, a configuration that can be shown to be safe under 
worst-case uncertainty estimates for object and manipulator configuration. 

3. Identify a grasp deproach configuration, a configuration which is safe for the manipulator 
grasping the part, given the uncertainty in the part's configuration after grasping and the uncer- 
tainty in configurations of nearby objects. 

4. Compute a path, from the manipulator's initial configuration to the grasp approach 
^^ configuration, assuming worst-case uncertainty. 

5. Identify a destination approach configuration, a configuration which is safe for the manipulator 
holding the object, given the uncertainty in the grasp configuration and the uncertainty of 
nearby objects. 

6. Compute a safe path from the grasp deproach configuration to the destination approach 
configuration for the manipulator and the grasped part, also assuming wort-case uncertainty. 

7. Identify a destination depproach configuration, a configuration which is safe for the 
manipulator, given the uncertainty of nearby objects. 

This formulation of the synthesis problem factors out the problems of approaching and deproach- 
ing both the nominal grasp configuration and the destination. For both of these problems, the use 
of sensory information to identify the actual state of the task and to accomodate to it is important 
[25] [30] [44]. When the uncertainty is small, the problem can be dealt with by ad hoc methods, 
e.g. opening the fingers very wide and relying on the grasping action to place the object and/or 
the manipulator in approximately the correct orientation [19]. The general problem of planning 
manipulator operations that are robust in the face of uncertainty is an important problem [30], but 
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beyond the scope of this paper. 

11. Summary 

This paper has presented an approach to the central geometric problems underlying die synthesis 
of "pick and place" motions for cartesian manipulators. The key technique in the approaches the use 
of explicit polyhedral representations of the configuration constraints on the manipulator. This repre- 
sentation permits the use of simple and powerful geometric operations to solve problems involving 
safe motions of the manipulator. In particular, the problems of finding grasp configurations and safe 
paths in the absence of uncertainty. 

The concepts of Configuration Space and Configuration Space Obstacle have played a central role 
in the approach to gross motion synthesis developed here. Similar concepts play an important role in 
die approach to compliant motion synthesis described in [30]. These concepts have also proven useful 
in other geometric applications [1] [2] [4] [45]. 
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Figure 15. The WRDGE is a convex polyhedron used to approximate the volume swept out by a cuboid 
aligned with the coordinate axes, as it rotates around the z axis, assuming the z axis does not penetrate the 
cuboid 



Appendix 1. A Polyhedral Approximation for Swept Volume 



The swept volume is the volume occupied by a polyhedron over a set of configurations, e.g. along 
some path. The swept volume over a range of translations can be computed using the CO xyz algo- 
rithm. In this appendix, we will limit our attention to computing a simple polyhedral approximation 
to the swept volume for rotations of a polyhedron around an arbitrary axis. This method is included 
here for completeness, it is not the best polyhedral approximation to the swept volume. 

The swept volume approximation described here returns a list of convex polyhcdra of two types: 
1. CYLINDER — a polyhedral approximation to a right circular cylinder. 
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figure 16. Computing a polyhedral approximation to the swept volume under pure rota- 
tion. 
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2. WEDGE — a polyhedral approximation to the volume swept out by a cuboid, aligned with 
the coordinate axes, as it rotates around the z axis, Figure 15. It assumes that the z axis does 
not penetrate the cuboid and that the rotation is less than *. 

The input is a polyhedron, J5, an axis of rotation which is the z axis of a reference frame and 0, 
the angle of rotation. The first step is to rotate the frame around z so that the x axis goes through 
the ccntroid of the projection of B on the (x, y)-plane of the frame. Compute an aligned bounding 
rectangular solid for B, RB(B)< whose dimensions are (Ax, Ay, Az). If the z axis does not pass 
through the object, then if < inax < * then simply return a WEDGE enclosing the swept volume. 
If the z axis penetrates RB{B), then if Ax > Ay, cut B using the planes x = ^ and x — zz Y k , 
and return a cylinder of radius y/2Ay whose height is Az and return the swept volumes of the pieces 
of B beyond the central area. The procedure is similar if Ay > Ax. Figure 16 illustrates this 
process. Here max is some user specified parameter, although it could be chosen to guarantee some 
kind of error bound. If > ma x* then divide die rotation into a set of successive rotations each 
returning a wedge* 
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